#include "RTthreads.h"

#include <string.h>
#include <pthread.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include "RTthreads.h"
#include "tools_share.h"
#include "test_share_mem.h"

struct cylic_thread_params ctp_fast_task;
struct once_thread_params otp_ECM_INIT;
struct waitsem_thread_params wtsem_slow_follow_task1;
struct waitsem_thread_params wtsem_slow_follow_task2;

#define PROGRM_MAIN_PATH    "EXO_MAIN_PATH" //系统的变量名称

int counterb=0;
void CylicThread_DATA_exchange(struct cylic_thread_params *f_p)
{
    counterb++;
    
    locksem(sbf.semid,USR_SEMID_RT_STATUS);
    BSet(sbf_pdata->rt_data.B01,B01_spi_enable);
    BRSet(sbf_pdata->rt_data.B01,B01_walk_auto_enable);
    BFlip(sbf_pdata->rt_data.B01,B01_walk_nagtive_enable);
    BSetV(sbf_pdata->rt_data.B01,B01_SERVO_init_success,0);
    sbf_pdata->rt_data.FSM_State=counterb;

    printf("%d,%d,%f,%f,%f,%f\n",sbf_pdata->rt_data.FSM_State,sbf_pdata->rt_data.error_STATUS
    ,sbf_pdata->test_dbl[0],sbf_pdata->test_dbl[1],sbf_pdata->test_dbl[2],sbf_pdata->test_dbl[3]);

    unlocksem(sbf.semid,USR_SEMID_RT_STATUS);
}

int counter1=0;
void CylicThread_Ctrl1(struct waitsem_thread_params *f_p)
{
    counter1++;
    locksem(sbf.semid,USR_SEMID_RT_SERVO_CTRL);
    for(int i=0;i<8;i++)
    {
        sbf_pdata->rt_data.model_Q[i]+=i*0.1;
    }

    printf("%f,%f,%f\n",
    sbf_pdata->rt_data.model_Q[1],
    sbf_pdata->rt_data.model_Q[2],
    sbf_pdata->rt_data.model_Q[3]);

    sleep(1);//大量运算导致USR_SEMID_RT_SERVO_CTRL被锁定1s，导致TestSharememRead读取数据时要等待信号量，从而造成timeout

    unlocksem(sbf.semid,USR_SEMID_RT_SERVO_CTRL);
}

int main() 
{
    int i;

//获取程序目录\环境变量：
	char * s = getenv(PROGRM_MAIN_PATH);
	memcpy(s_PROGRM_MAIN_PATH,s,strlen(s));

//建立共享内存：
    test_sharemem_server_build(&sbf,135);
    for(int i=0;i<8;i++)
    {
        sbf_pdata->rt_data.model_Q[i]=0;
    }


/*建立基频倍频线程START*/
	InitCylicThread(&ctp_fast_task);
	ctp_fast_task.thread_id = 4;
	ctp_fast_task.period_ns = 500*1000000;//10ms
	ctp_fast_task.cyclic_task = CylicThread_DATA_exchange;
	ctp_fast_task.ctrl = 0; // OnceThread_ECM _INIT启动完成后，再执行本线程的内容
	ctp_fast_task.timeout_warn=0;
	ctp_fast_task.CPU_idx = 1; //
	ctp_fast_task.priority=83;
    ctp_fast_task.bindsem_ftokid=256;

	wtsem_slow_follow_task1.thread_id = 5;
	wtsem_slow_follow_task1.waitsem_task = CylicThread_Ctrl1;
	wtsem_slow_follow_task1.CPU_idx = 2; //
	wtsem_slow_follow_task1.priority=75;
    bind_RTWaitSemTickThread(&ctp_fast_task,&wtsem_slow_follow_task1,3);//将wtsem_ctrl与ctp_fast_task以倍频绑定，占用semid0
	build_RTCylic(&ctp_fast_task);//只build ctp_fast_task，wtsem_ctrl会自动建立
	append_Cylic(&ctp_fast_task);



    *(ctp_fast_task.p_ctrl) = 1;//打开基频ctp_fast_task线程，同时会自动打开倍频CylicThread_Ctrl1、2

    int cnt=0;
    while (cnt<60000)
    {
        sleep(1);
        cnt++;
    }
    clearAllCylicThread();
    sleep(1);
    delete_all(&sbf);


    return 1;

}
